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Abstract 

In  this  report,  a  real-time  optimal  nonlinear  filter  is  developed.  This  fast  filter 
is  based  on  two  techniques:  (i)  splitting  of  the  convection  and  diffusion  operators, 
and  (ii)  tracking  of  important  windows.  Presented  is  an  explicit  scheme  using  the 
forward  characteristic-equation  and  the  fast  Gauss  transform.  The  domain  of  interest 
is  determined  adaptively.  Subroutines  of  the  software  code  are  briefly  described  and 
numerical  results  for  3-D  problems  are  given. 
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1  Introduction 

Nonlinear  filtering  is  the  process  of  computing  estimates  of  the  current  state  Xt  of  a  nonlinear 
stochastic,  or  noisy,  dynamic  system  (e.g.,  a  rapidly  maneuvering  target),  given  the  current 
measurements  which  are  obtained  as  nonlinear  functions  of  the  system  states  plus  noise. 
The  states  may  include  such  unknown  target  characteristics  as  position,  speed,  acceleration, 
aspect  angle,  etc.  The  relationship  between  measurements  and  target  states  is  modeled  by 
a  (generally  nonlinear)  measurement  equation  of  the  form  Zk  =  h{xk,Vk)  where  Vk  denotes 
the  noise  process.  The  expected  range  of  possible  behaviors  of  the  target  is  modeled  by 
Markov-state  transition  equations  of  the  form  Xt  =  b(xt,Wt)  where  Wt  is  another  noise 
process. 

There  are  military  situations  in  which  it  is  desirable  to  estimate  the  position,  velocity 
and  perhaps  acceleration  of  a  target  from  measurements  of  angle  but  not  range.  A  well- 
known  example  is  the  determination  by  a  submarine  of  planar  position  and  velocity  of  a 
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ship  from  passive  sonar  measurements,  because  the  submarine  commander  does  not  want  to 
reveal  his  presence  by  pinging.  In  air  warfare,  a  fighter  defending  against  a  raid  may  wish 
to  launch  a  missile  against  a  jammer  at  unknown  range,  but  should  not  do  so  unless  the 
jammer  s  position  and  velocity  can  be  estimated.  A  more  recent  problem  is  estimation  of 
tarpt  position,  velocity  and  acceleration  in  three  dimensions  from  angle  measurements  only 

either  with  a  passive  IR  receiver  or  a  jammed  radar  receiver  on  a  missile,  in  order  to  utilize 
optimal  guidance. 

The  extended  Kalman  filter  (EKF)  has  been  the  main  choice  in  most  approaches  to 
angle-only  tracking.  But  a  serious  limitation  of  EKF  application  to  angle-only  tracking  is 
the  nontrivial  nonlinearity  of  the  underlying  problem.  Its  mathematical  model  in  3D  can  be 
described  as  follows. 

Signal: 

dxi{t)  =  X2{t),  X3(t))dt  -|-  aidwi(t), 

dx2{t)  X2(t),  X3(^t'))dt  -{■  0'2dW2{t^, 

dxjit)  -  b3{xi{t),X2{t),X3{t))dt  +  asdwslty, 

Observation: 

Mk)  =  sign(x,(4))arccos  +  e,{k)v,{k), 

Z2{k)  =  arcsin  +  S,(k)v2(k), 

where  wi{t),W2{t),W3{t)  are  independent  Wiener  processes,  vi{k),V2{k)  are  standard  normal 
random  variables,  and  the  distribution  of  the  initial  state  Xi  (0),  0:2(0),  2:3(0)  is  given.  (Here  we 
consider  dynamic  systems  with  discrete  observations  because  in  most  cases  the  observational 
measurements  are  only  available  at  discrete  time  moments.) 

The  modified  polar  coordinates  (MFC)  introduced  by  Hoelzer  et  al  [9]  are  designed 
to  reduce  the  nonlinear  observations  to  linear  ones.  Unfortunately  in  doing  so  MFC  also 
transforms  the  signal  process.  Unless  the  latter  is  of  very  simple  nature,  the  MFC  transform 
makes  the  signal  system  practically  intractable. 

Much  more  perspective  approach  to  angle-only  tracking  is  based  on  optimal  nonlinear 
filtering.  The  optimal  nonlinear  filtering  theory  allows  to  compute  the  posterior  density  func¬ 
tion  TTk{x)  of  the  signal  process  2:(U)  =  (a:i(U), -2:2(4),  2:3(4))  given  observations  z(j),  j  <  k 
The  posterior  density  function  is  defined  by  7r,(2:)  =  Pkix)/ J  pkU)d^  where  the  so  called 
unnormahzed  filtering  density  pk{x)  can  be  formulated  in  terms  of  solutions  of  the  corre¬ 
sponding  Fokker-FIanck  equation  (see  the  next  section).  For  a  long  period  of  time  practical 
application  of  nonlinear  filtering  has  been  strained  by  numerical  difficulties  related  to  on-line 
solution  of  the  Fokker-Planck  equation. 

In  this  report,  we  propose  a  new  algorithm  for  the  target  tracking  problem,  mainly 
for  solving  the  corresponding  Fokker-FIanck  equation.  It  has  been  demonstrated  that  the 
techniques  used  in  the  new  algorithm  decrease  prediction  error  and  provide  a  more  accurate 
representation  of  the  target  bearings  as  compared  to  EKF.  On  the  other  hand,  the  algorithm 
is  also  fast,  in  that  its  calculations  need  only  0{N)  flops  per  time  step  where  N  is  the 
number  of  points  in  the  spatial  domain.  An  adaptive  method  is  used  to  update  the  domain 
of  interest  so  that  the  number  of  points  in  the  domain  is  kept  relatively  small.  Thus,  our 
approximation  of  the  optimal  nonlinear  filter  has  an  optimal  computational  complexity  for 
arbitrary  nonlinear  systems. 
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In  Section  2,  we  state  the  nonlinear  filtering  problem  in  a  more  general  and  rigorous  set- 
ting  and  formulate  its  theoretical  solution.  Sections  3  and  4  are  devoted  to  the  development 
of  the  fast  nonlinear  filter,  and  Section  5  to  a  brief  documentation  of  the  subroutines  of  the 
software.  Finally  numerical  e.xamples  are  given  in  Section  6. 


2  Statement  of  the  Nonlinear  Filtering  Problem 

Now  consider  the  dynamical  system  described  by  the  stochastic  differential  equation 

dX{t)  =  h{X{t))dt  +  a{X{t))dW{t),  t>0, 

A'(O)  ~  7ro(a;),  (1) 

and  the  discrete  observations  given  by 


z{k)  =  h{X{tk))  +  e{X{tk))V{k),  =  1, 2,  •  •  • ,  (2) 

where  ttq  :  .K"  is  the  initial  density,  6  :  .K"  iR",  and  :  iR”  ->  iR™  are  known 

vector-valued  functions,  a  :  ^  and  £  :  iR”  -)■  are  known  matrix-valued 

functions,  {VF(t)}t>o  is  a  standard  n-dimensional  Brownian  motion,  is  a  standard 

m-dimensional  white  Gaussian  sequence,  and  4  =  fcA  (A  >  0).  A'(O),  {IF(0}  and  {V'(A:)} 
are  assumed  to  be  independent,  and  functions  b,h,a,e  and  ttq  are  assumed  to  be  smooth 
enough  (satisfying  certain  regularity  conditions,  see  [14][17]). 

Let  /  =  f{x),x  6  jK”,  be  a  measurable  scalar  function  such  that  E\f{X{t))\'^  <  oo  for 
all  t  >  0.  Then  the  filtering  problem  for  (l)-(2)  can  be  stated  as  follows:  find  the  minimum 
variance  estimate  of  /(A(4))  given  the  measurements  z{l),  ■  •  • ,  z{k).  This  estimate  is  called 
the  optimal  filter  and  is  known  to  be 


The  optimal  filter  can  be  characterized  as  follows. 

Denote  by  Tt  the  solution  operator  for  the  Fokker-Planck  equation  corresponding  to  the 
state  process;  in  other  words,  u{t,x)  =  Tt(p{x)  is  the  solution  of  the  equation 


duit^x)  1  -A  d'^  ,  T  \ 

n  Q 

“  I]  t>0, 

u(0,a:)  =  (/?(x). 


(3) 


m 

where  {a{x)a{x)'^)fj,^  =  ^  cr^i(a;)(TM(a;)  is  the  ju-th  row  and  z/-th  column  entry  of  <7{x)a{xY , 

t=i 

and  hi,[x)  is  the  i/-th.  component  of  b{x). 

Define  the  unnormalized  filtering  density  pk{x),  for  x  G  IR^  and  k>0.  by 


Po(a:)  =  7ro(a:), 

Pk{x)  =  ak{x)TAPk-i{x), 
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where 

ak{x)  =  exp  |--(^(A:)  -  h{x)f  {e{x)e{xY)-^{z{k)  -  /z(x))  j  . 
Then  the  optimal  filter  f{k)  can  be  written  as  ([10]) 

fik)  -  Pk{x)f{x)dx 

/j?"  Pk{C}dt 

Remark  If  we  have  an  initial  observation  zq  at  time  t 
(unnormalized)  filtering  density  in  (4)  in  the  following  way; 

Po{x)  =  ao{x)Tro{x). 


(5) 


=  0,  we  can  modify  the  initial 


3  Splitting  of  Convection  and  Diffusion  Terms 


Our  objectwe  here  is  to  develop  recursive  numerical  algorithms  for  computing  the  optimal 
er  m  which  the  on-line  computation  is  as  simple  as  possible;  in  particular,  the  number  of 
computer  operations  at  each  time  step  should  be  proportional  to  the  number  of  grid  points 
where  the  filtering  density  is  numerically  defined.  The  starting  point  in  the  derivation  is 
e  equation  for  the  unnormahzed  filtering  density  in  the  general  nonlinear  model,  and  the 
approach  is  based  on  the  technique  known  as  operator  splitting. 

To  compute  the  unnormalized  filtering  density,  a  fast  Fokker-Planck  solver  is  needed.  In 
we  present  a  method  which  is  based  on  the  operator-splitting  technique  ([ll]fl5]) 
Similar  ideas  have  been  used  in  [8]  for  solving  the  Zakai  equation  arising  from  image  filtering.’ 

e  rst  assume  that  m  the  noise  term  of  (1)  and  the  covariance  matrix  ct  is  constant  and 
diagonal:  Then  the  Fokker-Planck  equation  (3)  becomes 


du{t,x)  1 


1  "  ^2  ”  (9 

~  2  ^  ~  i  >  0, 

P'=l  u  ' 


dt 

u(0,a:)  =  if{x)  . 


Its  solution  can  be  expressed  as 


re(^(x)  =  is[(^(ni))exp{-j^\v.6)(^^(5))ds}j  ,  (6) 

where  ^^(f)  is  a  stochastic  process  satisfying 

de(f)  =  +  diag(a,)dIT(f)  , 

F[e(0)  =  x]  =  1  . 

To  proceed  the  splitting  of  convection  and  diffusion  terms,  denote  by  TP  and  T'^  the 
solution  operators  of  the  equations  ^ 

du{t,x)  A  a  /  ^ 

f>o, 

u(0,x)  =  ip{x)  , 
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and 


du{t,x)  _  1 

dt  ~  2  dxl 

u(0,x)  =  ip{x)  , 

respectively.  In  fact,  the  two  solution  operators  can  be  expressed  explicitly  as 


{alu{t,x)),  t  >  0, 


t; 


and 


TM^)  =  EWC*(())I  =  /  exp  {  -  x: 

ai-'-anJR^  '•  ^ 

where  processes  ?7^(/)  (deterministic)  and  satisfy 


(3^./  -  Uuf 
2alt 


}^{y)dy , 


(7) 

(8) 


dy^{i)  =  7"(0)  =  :r  , 

and 

dC{t)  =  diag(a^)dH^(^),  (""(0)  =  x  . 

Then  it  can  be  shown  that  the  following  approximation  formulas  hold: 


Ta^P  =  T^r^<f  +  0{A^), 

(9) 

Ta^  =  +  0( A^)  , 

(10) 

Tap>  =  TlTiTl^  +  0{A^)  , 

2  2 

(11) 

Tap^  =  0(A")  . 

2  2 

(12) 

Therefore,  instead  of  solving  the  original  Fokker-Planck  equation,  we  only  need  to  com¬ 
pute  (7)  and  (8).  To  compute  (7),  we  only  need  to  solve  an  ordinary  differential  equation. 
To  compute  (8),  we  only  need  to  integrate  over  a  small  area  near  point  x,  especially  when 
the  noises  are  small.  In  the  case  of  large  noises,  the  multi-grid  method  ([7])  can  be  used 
to  achieve  linear  computational  complexity.  A  simpler  alternative  is  the  ADI  method,  and 
since  the  diffusion  equation  is  separable  in  this  case,  the  ADI  method  does  not  even  introduce 
further  errors  (for  solving  the  discretized  system). 

To  see  the  difference  between  the  exact  solution  and  the  approximate  solution  (by  split¬ 
ting),  we  note  that 


'd't  —  dE  Tf  a:))j  —  lE^(p  {r]{t^  ((f,  a;)))exp  C(^:  ^)))<^-5}]  7 

TlTfTl^{x)  =  TfTlip{ri{^,  x))  exp  {  “  ( ^  ■  b){r]{s,  a:))ds} 

=  at,  a^))))  exp{- jji'v -b)  (77(5,  C(f,  a:))))ds}]  exp  {-  b){r]{s,  .T))ds} 

=  dE[p[a\,  at,  v{\,  a^))))  exp  {  -  •  b){r,{s,  x))ds  -  ■  b)  {v{s;  ^  ({t,  r/(|,  x))))ds}] , 


where  for  obvious  reasons  we  have  written  =  ri{t,x),  C{t)  =  C{t,x),  and  = 

r]{t-,t',x),  the  last  being  the  process  77(f)  starting  (with  probability  1)  from  the  point  x  at 
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time  t .  Comparing  these  results  with  (6),  we  find  that  in  effect  our  approximation  is  to  split 
process  i{t)  into  two  simpler  processes:  a  deterministic  process  and  (up  to  a  constant) 
a  Wiener  process  C(t). 

In  general,  if  the  covariance  matrix  a  =  a{x)  is  not  constant  but  is  still  assumed  to  be 
diagonal  to  simplify  expressions,  i.e.,  cr^j,y[x)  —  S^yat^^x)^  we  may  rewrite  the  Fokker-Planck 
equation  in  the  following  form 


du{t,  x) 
dt 


d 

dxu 


u{t,  X 


)) 


i/=l 


where  ai^(x)  —  {ly  —  1,  •  •  •  ,n).  And  then  split  it  into  two  equations 


du{t^  x) 
dt 


n 


-E 


d 

dxu 


and 


du{t^  x) 
dt 


both  of  which  can  be  solved  in  linear  computational  complexity. 

In  this  general  situation,  the  three  solution  operators  T^,  and  Tf  can  also  be  expressed 
via  certain  stochastic  (or  even  deterministic)  processes.  Indeed,  similar  to  formulas  (6),  (7) 
and  (8),  we  have 


Tt<f{x)  =  iE[vp(e"(t))  exp  {  jf'  V  •  (a  -  6)(r(5))ds}j , 

TM^)  =  ¥^(r/"(t))  exp  {  jT'  V  •  (a  -  6)(r?"(s))ds}, 

and 

TMx)  =  «[v(C'(i))l . 

where  processes  and  (^(t)  satisfy 

dC(t)  =  (2S(C(t))  -  b(e(t)))dt  +  diag(<i„(r(i)))rfir(0,  f'(0)  =  x, 

dr,‘(t)  =  {Hr,’{t))  -  b(-n%t)))dt,  ,'(0)  =  X, 

and 

dC{t)  =  d{C{t))dt  +  diag(a4r(i)))c^W(t),  C"(0)  =  x, 

respectively. 

The  algorithm  by  splitting  the  convection  (or  drift)  term  from  the  (pure)  diffusion  term  as 
discussed  above  has  a  further  advantage  that  much  of  the  computation  in  (7)  and  (8)  or  their 
generalizations  can  be  performed  before  the  observations  are  available.  This  pre-calculation 
can  save  the  on-line  cost  and  further  speed  up  the  real  time  performance. 
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4  Adaptive  Domain  Updating 

Even  though  we  have  developed  optimal  solvers  for  the  Fokker-Planck  equation,  still  the 
nonlinear  filtering  problem  can  hardly  be  solved  in  real  time  (especially  for  large  state  di¬ 
mensions)  before  we  can  narrow  down  the  size  of  the  domain  in  which  the  Fokker-Planck 
equation  is  solved.  In  this  section,  we  present  a  windowing  technique  which  is  based  on  our 
convection-diffusion  splitting  framework. 

Assume  we  are  given  an  initial  set  Dq  of  “important”  points  which  are  chosen  according 
to  the  initial  filtering  density  po: 

Do  =  {x^(0),---,a;^(0)}. 

Usually  these  are  the  points  with  the  largest  values  of  po{x)  (or  with  the  most  important 
information  of  ^0(2^))!  Now  we  describe  how  to  efficiently  compute  the  “important”  points  in 
all  the  subsequent  time  steps  and  also  the  corresponding  values  of  the  unnormalized  filtering 
densities  at  those  points.  _ 

Let  L  be  a  positive  integer.  We  will  first  construct  an  enlarged  set  of  LN  candidates 
for  the  important  points  and  then  choose  from  them  the  “best”  N  points  according  to  the 
observational  probability  ak. 

For  simplicity,  we  assume  again  that  the  covariance  matrix  <t  is  constant  and  diagonal: 
o'fii.ix)  —  Our  algorithm  can  be  described  as  follows. 

•  Start  with  Do  =  {a;i(0),  •  •  • ,  x^(0)}  and  =po(4)(l  <  *  <  N). 

•  For  =  1,2,  •••,  assuming  Dk-i  =  {x^k  -  l),---,x^{k  -  1)}  and  pi_i{l  <  i  <  N) 
have  been  computed, 

(1)  for  i  =  1,  •  •  • ,  A,  solve  the  stochastic  integral  (or  differential)  equation 

X{t)  =  x\k  -  1)  +  f  b{X{s))ds  f  adW{s),  t  e  (0,  A]  , 

V  0  Jo 

with  L  different  sample  paths  of  the  Wiener  process  W(t),  including  the  trivial 
case  W{t)  =  0,  and  denote  by  x'd  the  solution  of  the  above  equation  at  time 
t  =  A  with  the  j-th  sample  path,  j  =  1,  •  •  • ,  L; 

(2)  determine  the  set  Dk  of  N  important  points  {a:^(/:),  •  •  • ,  x^{k)]  according  to  one 
of  the  following  two  criteria:  they  are  either  the  points  with  the  N  largest  values 
of  aJx^'O  for  all  i  and  j,  or,  for  each  i,  x\k)  has  the  largest  value  of  ak{x^’^)  for 

i  = 

(3)  for  i  =  1,  •  •  • ,  W,  compute 

Pk  =  o:kix\k))  Y,  0k^Pi-i  , 
je4 

where  p'J  is  computed  according  to  one  of  the  following  two  formulae: 

-  1)  -  b^{x^{k  -  1))A)^ 
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exp 


{-E 


p  -  1))' 


1^=1 


2a?,  A 


and  c4  0  •  1  ^  i  <  iV,  min(/?ji,’'',  >  r},  r  being  a  thresholding  tolerance. 


Since  the  domain  of  interest  is  adaptively  moving,  the  number  N  of  spatial  points  in  the 
domain  can  be  reduced  to  a  relatively  small  number,  even  in  three  or  higher  (n)  dimensions. 
In  general  the  number  of  spatial  points  is  exponential  in  n,  but  we  are  reducing  this  number 
for  each  of  the  n  dimensions.  Therefore  the  computational  cost  is  exponentially  reduced  in 
our  algorithm. 


5  Brief  Description  of  the  Subroutines 

The  complete  software  will  contain  two  parts:  one  using  an  explicit  scheme,  as  described 
above,  and  the  other  using  an  implicit  scheme,  which  will  be  addressed  later.  In  the  following 
we  explain  the  subroutines  of  the  e.xplicit  part.  Corresponding  source  code  is  attached  at 
the  end  of  the  report. 

advection.m 

a  function  for  solving  a  system  of  ordinary  differntial  equations 
cutsmall.m 

a  function  to  cut  small  numbers  for  thresholding  purpose 
func-b.m 

—  a  function  for  the  drift  term  b{x) 
func-db.m 

—  a  function  for  the  gradient  V  •  6(x) 
funcJi.m 

—  a  function  for  the  observation  function  h{x) 
func_p0.m 

—  a  function  for  the  initial  filtering  density  po(a:) 
obs_cor.m 

—  a  function  for  the  observational  correction  cxk{x) 
generate. m 

a  program  to  generate  the  (supposedly  unknown)  target  trajectory  X[t) 
and  the  corresponding  observations  z{k) 

initialize. m 

a  program  to  provide  initial  data  and  also  to  initialize  global  parameters 
onestep.m 

a  program  to  compute  the  filtering  density  Pk{x) 
peak.m 

—  a  function  for  locating  the  peak  of  the  filtering  density 
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plotting. m 

—  a  function  for  visualizing  the  tracking  process 
prior.m 

—  a  function  for  computing  the  prior  (transitional)  density 
run.m 

—  the  main  program 


6  Numerical  Examples 

To  illustrate  the  performance  of  the  above  algorithm,  let  us  consider  two  practical  examples. 
Here  we  take  T  =  1  in  the  above  algorithm. 

The  first  example  is  a  problem  of  estimating  the  altitude,  velocity,  and  constant  ballistic 
coefficient  of  a  vertically  falling  body,  given  measurements  taken  at  discrete  instants  of  time 
by  a  radar  that  measures  -range  (only)  in  the  presence  of  white  Gaussian  noise: 

Signal:  Xi(f)  =  -Xiit)  -  Xx{t))  +  aiWi{t), 

X2{t)  =  -Xl{t)X3{t)exp{—fXiit)}  +  a2W2{t), 

-^3(0  =  0  +  a^Wxiit)^ 

Observation:  z{k)  =  +  [Xi(4)  -  +  v{k), 

where  7  =  5- 10-^  =  10^  M  =  10^  =  450,  02  =  88,  =  5.6  •  10'®,  4  =  kA,  A  =  0.5, 

v[k)  ~  A^(0,  (8  •  10^)^),  and  the  initial  target  state  (Xi(0),  ^2(0),  ^3(0))  has  joint  density 
7ro(a;i,a:2,a:3)  =  ^  exp(-10-^(xi  -  3  •  10^)^  -  2.5  •  10-^(x2  -  2  •  lO'*)^  -  10'*(a;3  -  3  •  10-®)^), 
Co  being  the  normalizing  constant. 

In  our  experiments,  we  have  taken  r  =  30.0,  (Xi(0),  ^2(0),  ^3(0))  =  (3•10^2•10^,  10"^), 
xi(0)  =  2.1  •  10®  +  3  •  lOH  {0  <  i  <  10),  4(0)  =  1.84  •  10“  +  2  •  10®j  (0  <  i  <  10), 
^3(0)  =  3-10  ®  +  1.4  -10  “A;  (0  <  A;  <  7),  and  so  =  10  x  10  x  7  =  700.  Results  of  typical 
tracking  steps  are  shown  in  the  figures  at  the  end  of  this  report. 

Our  second  example  is  the  problem  of  tracking  a  noisy  Lorenz  system  with  angle-only 
observations: 


Signal: 


Observation: 


^i(A)  —  ci(X2(t)  —  Xi(f))  -|-  ctiHT(A), 

^2(0  =  r2Xi{t)  -  Xx{t)X3{t)  -  X2{t)  +  a2W2{t), 
^3{t)  —  Xi[t)X2{t)  —  r^X^iit)  -f-  a2,Wz{t)^ 
zi{k)  =  sign(X2(4))  arccos  ,  +  vi{k), 


Z2(k)  =  arcsin  _l_  x,J]A 


y/xf{t,)+xi{t,) 


where  n  =  10,  r2  =  28,  r3  =  8/3,  ci  =  0.045,  02  =  0.023,  03  =  0.012,  4  =  kA,  A  =  i, 
vx{k)  ~  fV(0,.64''),  V2ik)  ~  iV(0,.362),  and  the  initial  target  state  (Xi(0),  ^2(0),  ^3(0))  has 
joint  density  7ro(a:i,  12, 2:3)  =  ^exp(-25(a:i  -4.5)^  -  16(a:2  +  4.5)^  -  9(^3  -  19)^),  cq  being 
the  normalizing  constant. 

In  our  experiments,  we  took  T  =  4.0,  (^1(0),  ^2(0),  ^3(0))  =  (5, -5, 20),  a;j(0)  =  4  -f 
f  (0  <  f  <  10),  4(0)  =  -6  +  I  (0  <  i  <  10),  4(0)  =  19  +  I  (0  <  fc  <  10),  and  so 
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=  10  X  10  X  10  =  1000.  Results  of  typical  tracking  steps  are  shown  in  the  figures  at  the 
end  of  this  report. 

The  computational  cost  for  these  two  examples  (in  terms  of  CPU  seconds  and  FLOPS 
per  time  step)  are  given  in  Table  1.  (Note:  A  portion  of  CPU  time  for  the  Lorenz  example 
was  used  for  better  viualization.) 


Table  1;  Computational  complexity 


Problem 

N 

epu 

flops 

Ballistic 

Lorenz 

700 

1000 

0.065 

0.67 

80659 

135406 

In  conclusion,  the  filtering  algorithm  proposed  in  this  report  is  based  on  the  exact  optimal 
filter  and  so  applies  to  systems  with  high  nonlinearity  and  different  noise  levels.  On  the  other 
hand,  the  new  algorithm  is  fast,  in  that  the  involved  calculations  have  linear  comlexity  per 
time  step  and  the  number  of  points  at  which  the  calculations  are  performed  is  small.  In 
other  words,  we  have  developed  a  fast  optimal  nonlinear  filter. 


Appendix 

MATLAB  source  code  will  be  included  at  the  end  of  this  report. 
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Ballistic  Coefficient  X3(t) 


Observations  z(k) 


0 


5 


10 


15 

Time  t  — > 


_L_ 

20 


Velocity  X2(t) 


Altitude  X1{t) 


z2(k) 


%%%%%%%%%%%% 

1.  Ballistic 

cpul  =  0.0645' 
flops  1  =  80659 

cpu60=  3.88 
flops60  =  4839540 


gam  =  5e-5; 

H  =  le5;  %0;  %  2e5; 
M=  le5; 


Tfinal  =  30.0; 
Kfinal  =  60;  %30; 
Kmax  =  60;  %30 


nx  =  10; 
ny  =  10; 
nz  =  7; 

xa  =  2.1e5;  xb  =  3.1e5; 
ya  =  1.84e4;  yb  =  2.04e4; 
za  =  3e-5;  zb  =1. Ole-3; 

sigmaa=  [4.5e2,  8.8el,  5.6e-6]; 
delta  =  0.8e4; 

mO  =  [3e5,  2e4,  3e-5]; 
varO_inv  =  [le-9,  2.5e-7,  le4]; 
%varO_inv  =  [le-6,  2.5e-7,  le4]; 

XO=  [3e5,  2e4,  le-3]; 

Z0  =  0; 


%%%%%%%%% 

2.  Lorenz 

cpul  =  0.67 
flops  1  =  135406 

cpul28  =  86.08 
flops  128=  17331968 


ss  =  10; 
aa  =  28; 
bb  =  8/3; 

Tfinal  =  8.0; 

Kfinal  =  256; 

Kmax  =128;  %64;  %256; 


C.Rao 

almaak.results 


%le-6;  %le-4; 


nx  =  10; 
ny  =  10; 


nz  =  10; 
xa  =  4;  xb  =  6; 
ya  =  -4;  yb  =  -6; 
za  =  19;  zb  =  21; 


sigmaa  =  [0.045,  0.023,  0.012]; 
delta  =[0.64,  0.36]; 

m0  =  [4.5,  -4.5,  19]; 
varOJnv  =  [25,  16,  9]; 


C.Rao 

almaak.results 


XO  =  [5,  -5,  20]; 
Z0  =  [0,0]; 


%%%%%%%%%% 

3.  Sigma3D 

cpul  =  0.65 
flops  1  =  115394 

cpul50  =  97.75 
flopsl50=  17309100 

Tfinal  =  2.0; 

Kfinal  =  256;  Kt  =  Kfinal; 

Kmax  =  150  %  128;  %64;  %256; 

nx  =  12; 
ny  =  10; 
nz  =  8; 

xa  =  0.15;  xb  =  0.75; 
ya  =  0.30;  yb  =  0.60; 
za  =  0.14;  zb  =  0.30; 

sigmaa  =  [0.045,  0.023, 0.012]; 
%delta  =  [0.24, 0.12]; 
delta  =[0.64,  0.36]; 

mO  =  [0.45,  0.42,  0.20]; 
varOJnv  =  [100,  121,  64]; 
%varO_inv  =  [82,  100,  64]; 


%X0  =  [0.75,  -0.5,  -0.25]; 
X0=[3.6/4/2,  1./2,  1./4]; 
Z0=[0,0]; 


C.Rao 
run.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^%%%^ 


%% 

New  operator  splitting  method  for 

solving 

%% 

dX  =  b(X)dt  +  sigmaa(X)dW,  X(0)  = 

=  mO 

%% 

z (k)  =  h(X(t_k))  +  delta (k)v(k) 

%% 

Chuanxia  Rao 

%% 

January  1996 

%% 

January  1997 

%% 

May  1997 

%% 

Sep  1997 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^ 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% 

This  is  the  main  program. 

%%--- 

%% 

Dependencies : 

%% 

initialize .m 

%% 

moreinit .m 

%% 

generate .m 

%% 

onestep .m 

%% 

plotting. m 

%%--- 

- 

%%  Output: 

%%  graphs 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


clear; 


initialize;  %%  initial  data  and  parameters 

generate;  %%  to  generate  processes  X  and  Z 

figure;  axHndl=gca; 
set (axHndl ,  ... 

'XLim'  ,  [0  40]  ,  'YLim' ,  [-35  10] ,  'ZLim' ,  [-10  40]  ,  .  .  . 

'Drawmode',  'fast',  ... 

'Visible' , 'on' ,  ... 

'Next Plot' , 'add' ,  ... 

'View' , [-37.5,30] ) ; 
xlabel ( ' Z ' ) ; 
ylabel { 'X' ) ; 
zlabel ( ' Y' ) ; 

headO  =  line (  ... 

' color ' , ' g ' ,  ... 

' Marker 

'markersize ' , 25 ,  ... 

' erase ' , ' xor ' ,  ... 

'xdata' ,X(1,3) , 'ydata' ,X(1,1) , 'zdata' ,X(1,2) ) ; 
tailO  =  line {  ... 

' color ' , ' b ' ,  ... 

' LineStyle ... 

'erase' , 'none' ,  ... 

' xdata ',[],' ydata ',[],' zdata ',[]); 
headl  =  line {  ... 

' color ' , ' r ' ,  ... 

'Marker ... 

'markersize ', 25 ,  ... 

' erase ' , ' xor ' ,  ... 

' xdata ' , mO { 3 ) , ' ydata ' , mO ( 1 ) , ' zdata ' , mO ( 2 ) ) ; 
tail!  =  line (  ... 

' color '  ,  ' y ' ,  ... 

' LineStyle ... 

' erase ' , ' none ' ,  ... 

'xdata' , [] , 'ydata' , [] , 'zdata' , [] ) ; 


xyz  =  X ( 1 , : ) ; 


%  hold  on; 

YO  =  X ( 1 , : ) '  *  ones (1,2); 

Y1  =  mO { : )  *  ones (1,2); 

kstart=l ;  kstop=Kmax; 

%kstart=Kmax+l ;  ks top=Kf inal-1 ; 

%%global  time; 

count  =  flops; 
cpu  =  cputime; 
for  ktime  =  kstart : kstop, 

kl  =  ktime  +  1; 

%%  time  =  to  +  dt*ktime; 

fprintfd,  '%c',  '  step  '); 

fprintfd,  '%d\n',  ktime); 

onestep; 

kplot  =  ktime/plot_step ; 
if  ktime==l  |  kplot  ==  fix(kplot), 
xyz  =  X(kl, : ) ' ; 

YO  [xyz  YO  (  :  ,  1 )  ]  ; 

set (headO, 'xdata^ , YO (3 , 1) , 'ydata' , YO (1 , 1 ) , ' zdata' , YO (2, 1) ) 

set{tailO, ' xdata ^ YO ( 3 , 1 : 2 ) , 'ydata ' , YO (1 , 1 : 2 ) , ' zdata ' , YO ( 2 , 1 : 2 ) ) 

drawnow; 

xyz  =  peak(pk); 

Y1  =  [xyz  Yl(:,l:2)]  ; 

set(headl,  'xdata' ,Y1(3,1)  ,  'ydata' ,Y1(1,1) ,  'zdata' ,Y1(2,1) ) 
set(taill, 'xdata' ,Y1(3,1:2) , 'ydata' ,Y1(1,1:2) , 'zdata' ,Y1(2,1:2) ) 
drawnow; 

%  plotting (xyz ,  pk) ; 

end 

end 

%  hold  off; 

count  =  flops  -  count; 

cpu  =  cputime  -  cpu; 

cpu_onl  =  cpu  /  (kstop-kstart+1) 

flops_onl  =  count  /  (kstop-kstart+1) 


C.Rao 

ballistic/run.m 


%% 

New  operator  splitting  method  for 

solving 

%% 

dX  =  b(X)dt  +  sigmaa(X)dW,  X(0)  = 

=  mO 

%% 

z(k)  =  h(X(t_k))  +  delta(k)v(k) 

%% 

Chuanxia  Rao 

%% 

January  1996 

%% 

January  1997 

%% 

September  1997 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%^^ 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% 

%%-- 
%% 

%% 

%% 

%% 

%% 

%% 

%% - 

%%  Output: 

%%  graphs 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
clear; 


This  is  the  main  program. 

Dependencies : 
initialize .m 
moreinit .m 
generate . m 
onestep.m 
plotting .m 


initialize; 

generate; 


%%  initial  data  and  parameters 
%%  to  generate  processes  X  and  Z 


figure; 

xyz  =  X ( 1 , ; ) ; 
plotting (xyz ,  pk) ; 
kstart=l;  kstop=Kmax; 

%kstart=Kmax+l ;  kstop=Kf inal-1 ; 

count  =  flops; 
cpu  =  cputime; 

%%global  time; 

for  ktime  =  kstart : kstop , 

kl  =  ktime  +  1; 

%%  time  =  to  +  dt*ktime; 

fprintfd,  '%c' r  '  step  '); 
fprintfd,  '%d\n',  ktime); 


onestep; 

kplot  =  ktime/plot_step ; 
if  ktime==l  |  kplot  ==  fix{kplot) , 
xyz  =  X{kl, : ) ; 
plotting (xyz ,  pk) ; 

end 

end 

count  =  flops  -  count; 

cpu  =  cputime  -  cpu; 

cpu__onl  =  cpu  /  (kstop-kstart  +  1) 

flops_onl  =  count  /  (kstop-kstart+1 ) 


ballistic/initialize.m 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^ 
%%  Initial  data  for  the  cont.-discr.  model: 

%%  dX  =  b(X)dt  +  sigmaa*dW 

%%  X(0)  N(mO,varO) 

z(k)  =  h(X(t_k))  +  delta*v(k) 


%%  Chuanxia  Rao 
%%  Februay  1996 
%%  Februry  1997 
%%  July  1997 


%%  Called  in: 

%%  all  other  parts 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


global  dim  dim_obs 
global  nx  ny  nz 
global  xa  ya  za 
global  xb  yb  zb 
global  XX  yy  zz 
global  xendO  yendO  zendO^ 
global  xendl  yendl  zendl 
global  mO  varO_inv 
global  gam  Msquared  H 


global  dt  dt2 
global  denom 
global  denom_obs 
%global  nxpl  nypl  nzpl 


global  substeps 
global  small  %large 
global  resol_vert 


disp('  Initialization 
dim  =  3 ; 
dim_obs  =  1 ; 

Tfinal  =  30.0; 

Kfinal  =  60;  %30; 

Kmax  =  60;  %30 
Kt  =  Kfinal; 

nx  =  10; 
ny  =  10; 
nz  =  7  ; 

xa  =  2.1e5;  xb  =  3.1e5; 
ya  =  1.84e4;  yb  =  2.04e4; 
za  =  3e-5;  zb  =  1. Ole-3; 

xendO  =  0;  xendl  =  3.1e5; 
yendO  =  0;  yendl  =  2.04e4; 
zendO  =  3e-5;  zendl  =  l.Ole-3; 

%Nx  =  [nx,ny,nz] ; 

%Lx  =  [3,2,1] ; 

%Lx  =  [1,1,1];  %%  bandwidth  of  local  propogation: 

%%  It  depends  on  (dx*dx) / (dt*sigmaa^2 ) . 

plot_step  =  1;  %2;  %%  plot  in  every  plot_step  steps 

resol_vert  =  0.005;  %%  for  plotting  target  position 


%%  threshold  for  positive  probability 


substeps  =  1; 
small  =  le-16; 


C.Rao 

ballistic/initialize.m 


%large  = 


-log (small) ; 


%sigmaa  =  [0.0,  0.0,  0,0]; 
sigmaa  =  [4.5e2,  8.8el,  5.6e~6] 
%delta  =  0.0; 
delta  =  0.8e4; 


%% 

%% 


coefficients  in  signal  noise 
%le-6;  %le-4; 

coefficients  in  observ.  noise 


mO  =  [3e5,  2e4,  3e-5] ;  %%  initial  mean 

var0_inv  =  [le-9,  2.5e-7,  le4] ;  %%  initial  variance,  its  inverse 
%var0_inv  =  [le-6,  2.5e-7,  le4] ; 

XO  =  [3e5,  2e4,  le-3] ; 

ZO  =  0; 


gam  =  5e-5; 

H  =  le5;  %0;  %  2e5; 

M  =  leS; 

%% 

%%  Further  initialization: 
%%  --no  changes  needed  -- 
%% 


Msquared  =  M*M; 
dt  =  Tf inal/Kf inal ; 
dx  =  (xb-xa) /nx; 
dy  =  {yb-ya)/ny; 
dz  =  (zb-za)/nz; 

dt2  =  dt/2; 

%dt4  =  dt/4; 
nxpl  =  nx  +  1 ; 
nypl  =  ny  +  1; 
nzpl  =  nz  +  1; 


denom_obs  =  2  *  delta. ^2;  %%  a 
denom  =  {2*dt)  *  sigmaa. ^2;  %%  a 
%const_norm  =  sigmaa...  *  sqrt (2*pi*dt ) ^dim; 


row  vector, 
row  vector 


not  a  diagonal  matrix 


disp{'  Setting-up  of  initial  density  and  observation  function  ...') 

XX  =  xa  +  dx  *  {0:nx); 

yy  =  ya  +  dy  *  (0  :ny)  ; 

zz  =  za  +  dz  *  (0:nz); 

pk  =  func_p0(xx,  yy,  zz) ; 

%  pk  =  cutsmall (pk) ; 


xld  =  XX ; 


for 

for 

for 


end 

end 

end 


yld  =  yy; 
zld  =  zz; 

XX  =  zeros (nxpl, nypl, nzpl) ; 
yy  =  xx; 
z  z  =  yy  ; 
i-1 :nxpl , 
j=l :nypl, 
k=l :nzpl , 

XX ( i , j , k)  =  xld(i) ; 
yy (i/ j ,k)  =  yld( j ) ; 
zz (i, j , k)  =  zld(k) ; 


pO  =  func_pO (x, y, z ) 
pO  =  func_pO {x,y, z) 
x,y,z  are  vectors. 
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function 
%  usage: 
%  where 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  The  initial  density  function 


%%  Chuanxia  Rao 
%%  Februry  1997 
%%  May  1997 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^ 
%%  Called  in: 


%%  moreinit.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


global  mO  varO_inv 
%  temp  =  sqrt  (2*pi) ; 

%  tempo  =  temp*sqrt (varO_inv(l) *var0_inv(2) *var0_inv(3) ) 

for  i=l : length (x) , 

tempi  =  (x(i) -mO (1) ) ^2  *  varO_inv(l); 
for  j=l : length(y) , 

temp2  =  (y(j)-m0(2)  )'^2  *  var0_inv(2); 
for  k=l : length ( z ) , 

temp3  =  ( z (k) -mO { 3 ) ) ^2  *  var0_inv(3); 
temp3  =  tempi  +  temp2  +  temp3 ; 
pO(i,j,k)  =  exp(  -temp3/2  ); 

end 

end 

end 

%  pO  =  pO  /  tempo ; 

pO  =  pO  /  max  (  max  (  max  (pO  ,  [  ]  ,  3  )  ,  [  ]  ,  2  ),[],!  )  ; 
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[bl,b2,b3]  =  func_b(x,y, z) 

[bl,b2,b3]  =  func_b(x,y, z) 
x,y,z  and  bl , b2 , b3  are  of  the  same  size. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%% 

%% 

%% 

%% 

%% - 

The  signal  propogation  function 
Chuanxia  Rao 

Februry  1997 

August  1997 

%% 

Called  in: 

%% 

generate .m 

%% 

advection .m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

global 

gam 

bl  =  -y; 

b2  =  -exp(“gam*x)  .*  y.^2  .*  z; 
b3  =  zeros ( size (y) ) ; 

% 

b  =  [bl,  b2,  b3] ; 

function 
%  usage: 
%  where 
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result  =  func_db (x,y, z) 
result  =  func_db(x,y, z) 
x,y,z,  and  result  are  of  the  same  size. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  The  scalar  function  Delta  *  b 

%%  Chuanxia  Rao 

%%  August  1997 

%%  May  1997 

%% - 

%%  Called  in: 

%%  prior. m  (&  onestep.m) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


function 
%  usage: 
%  where 


global  gam 

if  (size (x) ==size (y)  &  size (y) ==size ( z ) ) , 

%  dbl  =  zeros (size (x) ) ; 

%  db2  =  -2  *  exp{-gam*x)  .*  y  .*  z; 

%  db3  =  zeros (size (z )) ; 

%  db  =  dbl  +  db2  +  db3  ; 

result  =  -2  *  exp(-gam*x)  .*  y  .*  z; 

else 

disp{'  Warning:  x,  y,  and  z  have  different  sizes  ?!') 
result  =  zeros (size (y) ) ; 

end 

%nl  =  length(x); 

%n2  =  length (y ); 

%n3  =  length ( z); 

%  dbl  =  zeros (nl , n2 , n3 ) ; 

%  db2  =  zeros (nl , n2 , n3 ) ; 

%  db3  =  zeros (nl , n2 , n3 ) ; 

%  db  =  zeros (nl , n2 , n3 ) ; 

%%  db  =  dbl  +  db2  +  db3  ; 

%%  dbl  =  db  /  3; 

%%  db2  =  dbl; 

%%  db3  =  dbl; 
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function  hh  =  func_h {x, y, z ) 

%  usage:  hh  =  func_h(x,y, z) 

%  where  x,y,z  and  hh  are  of  the  same  size. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  The  observation  function 

%%  Chuanxia  Rao 

%%  August  1997 

%%  April  1997 

%% - 

%%  Called  in: 

%%  obs_cor.m 

%%  generate. m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
global  Msquared  H 

if  (size (x) ==size (y)  &  size (y) ==size (z) )  , 
temp  =  Msquared  +  (x  - 
hh  =  sqrt(temp); 

else 

disp('  Warning:  x,  y,  and  z  have  different  sizes  ?!') 

end 


PS 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
Initial  data  for  the  cont.-discr.  model: 
dX  =  b(X)dt  +  sigmaa*dW 
X(0)  --  N(mO,varO) 
z(k)  =  h(X(t_k))  +  delta*v(k) 


%%  Chuanxia  Rao 

%%  Februay  1996 

%%  Februry  1997 

%%  July  1997 

%% - 

%%  Called  in: 

%%  all  other  parts 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%% 

%% 

%% 

%% 


global 

global 

global 

global 

global 

global 

global 

global 


dim  dim_obs 
nx  ny  nz 
xa  ya  za 
xb  yb  zb 
XX  yy  zz 

xendO  yendO  zendO' 
xendl  yendl  zendl 
mO  varO_inv 


global  dt  dt2 
global  denom 
global  denom_obs 
%global  nxpl  nypl  nzpl 


global  substeps 
global  small  %large 
global  resol_vert 


global  ss  aa  bb 

%%  constants  in  the  Lorenz's  system. 


ss  =  10; 
aa  =  28; 
bb  =  8/3; 

disp( '  Initialization 
dim  =  3 ; 
dim_obs  =  2 ; 

Tfinal  =  8.0; 

Kfinal  =  256;  Kt  =  Kfinal; 
Kmax  =  128;  %64;  %256; 

%Kmax  =  Kfinal  -  1; 

%Kmax  =  Kfinal  /  4; 

nx  =  10; 
ny  =  10; 
nz  =  10; 

xa  =  4 ;  xb  =  6 ; 
ya  =  -4;  yb  =  -6; 
za  =  19;  zb  =  21; 

%nx  =  50; 

%ny  =  50; 

%nz  =  30; 

%xa  =  -0.25;  xb  =  0.75; 

%ya  =  -0.40;  yb  =  0.60; 

%za  =0.0;  zb  =0,30; 


xendO  =  -35;  xendl  =  10; 
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yendO  =  -10; 
zendO  =  0; 


yendl 

zendl 


40; 

40; 


%xa  =  [-0.95,  -0.6,  -0.3]  ; 
%xb  =  [0.75,  0.6,  0.3]  ; 

%Nx  =  [nx,ny,nz]; 

%Lx  =  [3,2,1] ; 

%Lx  =  [1,1,1] ; 


%%  bandwidth  of  local  propogation: 

%%  It  depends  on  {dx*dx)  /  (dt*sigmaa'^2 )  . 


%sigmaa  =  [0.0,  0.0,  0.0]; 
sigiuaa  =  [0.045,  0.023,  0.012]; 
%delta  =  [0.0,  0.0] ; 
delta  =  [0.64,  0.36] ; 

%delta  =  [pi,  pi/2]; 

mO  =  [4.5,  -4.5,  19] ; 
var0__inv  -  [25,  16,  9]  ; 


%%  coefficients  in  signal  noise 
%%  coefficients  in  observ.  noise 

%%  initial  mean 

%%  initial  variance,  its  inverse 


%  X0{l)=rand*35-30; 

%  XO (2 ) =rand*40-5; 

%  X0(3)=:rand*30  +  5; 

XO  =  [5,  -5,  20]  ; 

%X0  =  [-5,  20,  5];  %%  only  one  cycle  ?! 

%X0  =  [3. 6/4/2,  1./2,  1./4] ; 

ZO  =  [0,0]; 


plot_step  =  1;  %2; 

%% 

resol_vert  =  0.01; 

%% 

substeps  =  1; 

small  =  le-16; 

%large  =  -log(small); 

%% 

plot  in  every  plot_step  steps 
for  plotting  target  position 


threshold  for  positive  probability 


%% 

%%  Further  initialization: 

%%  --  no  changes  needed  -- 

%% 

dt  =  Tf inal/Kf inal ; 
dx  =  (xb-xa)/nx; 
dy  =  {yb-ya)/ny; 
dz  =  (zb~za)/nz; 

dt2  =  dt/2; 

%dt4  =  dt/4; 
nxpl  =  nx  +  1 ; 
nypl  =  ny  +  1; 
nzpl  =  nz  +  1; 

denom_obs  =  2  *  delta. ^2;  %%  a  row  vector,  not  a  diagonal  matrix 

denom  =  (2*dt)  *  sigmaa.^2;  %%  a  row  vector 

%const_norm  =  sigmaa...  *  sqrt  (2*pi*dt )  "^dim; 

disp('  Setting-up  of  initial  density  and  observation  function  ...') 

XX  =  xa  +  dx  *  ( 0 : nx ) ; 

yy  =  y a  +  dy  *  ( 0 : ny ) ; 

zz  =  za  +  dz  *  {0:nz); 

pk  =  func_p0{xx,  yy,  zz); 

%  pk  =  cutsmall (pk) ; 


xld  =  XX ; 
yld  =  yy; 
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zld  =  zz; 

XX  =  zeros (nxpl,nypl,nzpl)  ; 
yy  =  XX ; 


zz  =  yy; 
for  i=l:nxpl, 
for  j=l:nypl, 
for  k=l:nzpl, 

XX ( i , j , k ) 

yy(i. j /k) 

zz (i, j ,k) 

end 

end 

end 


xld(i) ; 
yld(  j  )  ; 
zld(k) ; 


%  [hhl,  hh2]  =  func_h(xx,  yy,  zz)  ; 

%%  hhl  =  cutsmall (hhl) ; 

%%  hh2  =  cutsmall (hh2 ) ; 


C.Rao 
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function  pO  =  func_pO (x, y , z ) 

%  usage:  pO  =  func_jD0  (x, y,  z ) 

%  where  x,y,z  are  vectors. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  The  initial  density  function 

%%  Chuanxia  Rao 

%%  Februry  1997 

%%  May  1997 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Called  in: 

%%  moreinit.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

global  mO  varO_inv 

%  temp  =  sqrt (2*pi) ^3 ; 

%  tempo  =  temp*sqrt  {varO__inv(l)  *var0_inv(2)  *var0_inv(3)  )  ; 

for  i=l : length (x) , 

tempi  =  (x(i) -mO  (1)  ) '^2  *  varO_inv(l)  ; 
for  j=l : length{y) , 

temp2  =  {y ( j ) -mO (2) ) "2  *  var0_inv(2); 
for  k=l : length ( z ) , 

temp3  =  (z (k) -mO (3) ) ^2  *  var0_inv(3); 
temp3  =  tempi  +  temp2  +  temp3; 
pO{i,j,k)  =  exp(  -temp3/2  ); 

end 

end 

end 

%  pO  =  pO  /  tempo ; 

pO  =  pO  /  max  (  max  (  max  (pO  ,  [  ]  ,  3  )  ,  [  ]  ,  2  ),[],!  )  ; 


function 
%  usage: 
%  where 
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[bl,b2,b3]  =  func_b (x, y , z ) 

[bl,b2,b3]  =  func__b(x,y,  z) 
x,y,z  and  bl,b2,b3  are  of  the  same  size. 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  The  signal  propogation  function 

%%  Chuanxia  Rao 

%%  Februry  1997 

%%  May  1997 

%% - 

%%  Called  in: 

%%  generate. m 

%%  advection.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


global  ss  aa  bb 


bl  =  ss  (y  -  x)  ; 
b2  =  -x.*z  +  aa*x  ~  y; 
b3  =  x.*y  -  bb*z; 


b  =  [bl,  b2,  b3]  ; 


function 
%  usage: 
%  where 


C.Rao 

lorenz/func_db.m 

result  =  func_db {x,y, z ) 
result  =  func_db(x,y, z) 

x,y,z,  and  result  are  of  the  same  size. 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  The  scalar  function  Delta  *  b 

%%  Chuanxia  Rao 

%%  July  1997 

%%  May  1997 

%% - 

%%  Called  in: 

%%  prior. m  (&  onestep.m) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


global  ss  bb 


%if  (size (x) ==size (y)  &  size (y) ==size ( z ) ) , 
dbl  =  -ss  *  ones (size (x) ) ; 
db2  =  -  ones (size (y) ) ; 
db3  =  -bb  *  ones ( size ( z )) ; 
result  =  dbl  +  db2  +  db3 ; 

%else 

%  disp ( '  Warning:  x,  y,  and  z  have  different  sizes  ?! ') 

%  result  =  zeros (size (x) ) ; 

%end 
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function  [hl,h2]  =  func„h (x, y , z ) 

%  usage:  [hl,h2]  =  func_h (x, y, z ) 

%  where  x,y,z  and  hi , h2  are  of  the  same  size. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  The  observation  function 

%%  Chuanxia  Rao 

%%  April  1997 

%%  May  1997 

%%  Jul  1997 

%% - 

%%  Called  in: 

%%  obs_cor.m 

%%  generate. m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

if  (size(x)==size(y)  &  size (y) ==size ( z ) ) , 

[ll,inm,nn]  =  size(y); 
for  i=l:ll, 
for  j  =  l:mm, 
for  k=l:nn, 

templ2  =  x(i,j,k)'^2  +  y(i,j,k)'^2; 
if  templ2==0, 

hl(i,j,k)  =  0; 

h2(i,j,k)  =  asin(  sign ( z ( i , j , k) )  ); 

else 

argl  =  x{i,j,k)  /  sqrt ( templ2 ) ; 

tempi  =  sign(y (i, j ,k) )  *  acos(argl); 

hi (i, j , k)  =  tempi ; 

temp  =  templ2  +  z(i,j,k)^2; 

arg2  =  z(i,j,k)  /  sqrt(temp); 

h2{i,j,k)  =  asin(arg2); 


disp{'  Warning:  x,  y,  and  z  have  different  sizes 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Initial  data  for  the  cont.-discr.  model: 

%%  dX  =  b(X)dt  +  siginaa*dW 

%%  X(0)  -  N{mO,varO) 

%%  z(k)  =  h(X(t_k))  +  delta*v(k) 


%%  Chuanxia  Rao 

%%  Februay  1996 

%%  Februry  1997 

%%  July  1997 

%% - 


%%  Called  in: 

%%  all  other  parts 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


global  dim  dim_obs 
global  nx  ny  nz 
global  xa  ya  za 
global  xb  yb  zb 
global  XX  yy  zz 
global  xendO  yendO  zendO' 
global  xendl  yendl  zendl 
global  mO  varO___inv 


global  dt  dt2 
global  denom 
global  denom_obs 
%global  nxpl  nypl  nzpl 


global  substeps 
global  small  %large 
global  resol_vert 


disp('  Initialization 
dim  =  3 ; 
dim_obs  =  2 ; 


Tfina 

1  = 

2 

.0; 

Kfinal  = 

2 

56; 

Kt  = 

Kfinal ; 

Kmax 

=  150 

%128;  %64;  %256 

*  / 

%Kmax 

=  Kf 

inal 

-  1; 

%Kmax 

=  Kf 

inal 

/  4; 

nx  = 

12; 

ny  = 

10; 

nz  = 

8; 

xa  = 

0.15 

; 

xb 

=  0. 

75; 

ya  = 

0.30 

; 

yb 

=  0. 

60; 

za  = 

0.14 

/ 

zb 

=  0. 

30; 

%nx  = 

50; 

%ny  = 

50; 

%nz  = 

30; 

%xa  = 

-0. 

25; 

xb  = 

0.75; 

%ya  = 

-0. 

40; 

yb  = 

0.60; 

%za  = 

0.0 

/ 

zb 

=  0. 

30; 

xendO 

=z  - 

0 

.95; 

xendl  = 

0. 

75; 

yendO 

=  _ 

0 

.60; 

yendl  = 

0. 

60; 

zendO 

=  - 

0 

.30; 

zendl  = 

0. 

30; 

%nx  = 

72; 

%60; 

%40; 

%80; 

%ny  = 

60; 

%45; 

%30; 

%60; 

%nz  = 

30; 

%24; 

%20; 

%30; 

%xa  = 

[-0 

95, 

-0.6, 

-0.3 

]  ; 

%xb  = 

[0.75,  0.6, 

%Nx  = 

[nx, ny, nz ] ; 

%Lx  = 

[3,2,1] ; 

%Lx  = 

[1,1,1] ; 

0.3]  ; 


plot_step  =  2;  %1; 
resol_vert  =  0.01; 
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%%  bandwidth  of  local  propogation: 

%%  It  depends  on  {dx*dx)  /  (dt*sigmaa"'2 )  . 

%%  plot  in  every  plot_step  steps 
%%  for  plotting  target  position 


substeps  =  1; 

small  =  le-16;  %%  threshold  for  positive  probability 

%large  =  -log(small); 


%sigmaa  =  [0 . 0 ,  0 . 0  ,  0 . 0] ;  %%  coefficients  in  signal  noise 

sigmaa  =  [0.045,  0.023,  0.012]; 

%delta  =  [0.0,  0.0];  %%  coefficients  in  observ.  noise 

%delta  =  [0.24,  0.12] ; 
delta  =  [0.64,  0.36] ; 

mO  =  [0.45,  0.42,  0.20];  ^  %%  initial  mean 

var0_inv  =  [100,  121,  64];  %%  initial  variance,  its  inverse 

%var0_inv  =  [82,  100,  64]; 

%X0  =  [0.75,  -0.5,  -0.25] ; 

XO  =  [3 .6/4/2,  1./2,  1./4] ; 

ZO  =  [0,0] ; 


%% 

%%  Further  initialization: 

%%  --  no  changes  needed  -- 

%% 

dt  =  Tf inal/Kf inal ; 
dx  =  (xb-xa) /nx; 
dy  =  (yb-ya)/ny; 
dz  =  (zb-za)/hz; 

dt2  =  dt/2; 

%dt4  =  dt/4; 
nxpl  =  nx  -f-  1; 
nypl  =  ny  +  1; 
nzpl  =  nz  +  1 ; 

denom_obs  =  2  *  delta. ^2;  %%  a  row  vector,  not  a  diagonal  matrix 

denom  =  {2*dt)  *  sigmaa. ^2;  %%  a  row  vector 

%const_norm  =  sigmaa...  *  sqrt  (2*pi*dt ) '^dim; 

disp('  Setting-up  of  initial  density  and  observation  function  ...M 
XX  =  xa  +  dx  *  (0:nx); 

yy  =  ya  +  dy  *  {0:ny) ; 

zz  =  za  +  dz  *  (0 :nz) ; 

pk  =  func_p0 (xx,  yy,  zz)  ; 

%  pk  =  cut small (pk)  ; 

xld  =  XX ; 
yld  =  yy; 
zld  =  zz; 

XX  =  zeros (nxpl, nypl, nzpl) ; 
yy  =  XX ; 

zz  =  yy; 

for  i=l:nxpl, 
for  j=l:nypl. 
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for  k=l:nzpl, 

xx(i, j , k) 
yy{i/ j ,k) 
zz (i, j , k) 

end 

end 


end 


xld(i) ; 
yld(j) ; 
zld (k) ; 


[hhl,  hh2]  =  func„h(xx,  yy,  zz); 
hhl  =  cutsmall (hhl ) ; 
hh2  =  cutsmall (hh2); 


%% 

%% 
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function 
%  usage: 
%  where 


pO  =  func_pO (x,y, z) 
pO  =  func_pO (x,y, z) 
x,y,z  are  vectors. 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  The  initial  density  function 

%%  Chuanxia  Rao 

%%  Februry  1997 

%%  May  1997 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Called  in: 

%%  moreinit.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


global  mO  varO_inv 


%  temp  =  sqrt (2*pi) ^3; 

%  tempo  =  temp*sgrt (varO_inv(l) *var0_inv(2) *var0_inv(3) ) ; 

for  i=l : length (x) , 

tempi  =  (x{i) -mO (1) ) ^2  *  varO_inv(l) ; 
for  j=l : length (y) , 

temp2  =  (y  ( j  ) -mO  (2)  )  ^^2  *  var0_inv(2); 
for  k-1 : length (z) , 

temp3  =  (z  (k) -mO  (3)  )  ^^2  *  var0_inv(3); 
temp3  =  tempi  +  temp2  +  temp3 ; 
pO(i,j,k)  =  exp(  -temp3/2  ); 

end 

end 

end 

%  pO  =  pO  /  tempo ; 

pO  =  pO  /  max (  max (  max (pO ,  [ ] , 3 ) ,  [ ]  , 2  ),[],!  )  ; 


function 
%  usage: 
%  where 
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[bl,b2,b3]  =  func_b(x,y, z) 

[bl,b2,b3]  =  func_b(x,y, z) 
x,y,z  and.  bl,b2,b3  are  of  the  same  size. 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  The  signal  propogation  function 

%%  Chuanxia  Rao 

%%  Februry  1997 

%%  May  1997 

%% - 

%%  Called  in: 

%%  generate,  m 

%%  advection.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%for  j =1 : length (y) , 

%for  k=l : length ( z ) , 

%%  bl (1: length (x) ,  j,  k)  =  200*y(j)^3  +  50*z(k)  -  50; 

%  bl  (1:  length  (x)  ,  j,  k)  =  -  200*y(j)'^3  +  50*z(k); 

%end 
%end 

%  bl  =  200*y.'"3  +  50*z  -  50; 

bl  =  -200*y.^3  +  50*z; 
bl  =  bl/2; 


% 

% 

% 


b2  =  1./2  *  ones ( size (bl )) ; 
b2  =  -l,/2  *  ones ( size (bl )) ; 

b3  =  1./4  *  ones ( size (bl )) ; 
b3  =  -1./4  *  ones ( size (bl )) ; 

b  = 


[bl,  b2,  b3]; 


C.Rao 

sigma3D/func_db.m 


function 
%  usage: 
%  where 


result  =  func_db(x,y, z) 
result  =  func_db(x,y, z) 

x,y,z,  and  result  are  of  the  same  size. 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^ 


%%  The  scalar  function 

%%  Chuanxia  Rao 


Delta 


%%  July  1997 

%%  May  1997 

%% - 


9- 

•o 


%%  Called  in: 

%%  prior, m  (&  onestep.m) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


if  {size(x)==size(y)  &  size (y) ==size ( z ) )  , 
result  =  zeros (size ( z) )  ; 

else 

disp('  Warning:  x,  y,  and  z  have  different 
result  =  zeros (size (x) ) ; 

end 


%nl  =  length (x); 
%n2  =  length (y); 
%n3  =  length ( z ) ; 


O' 

"o 

dbl 

=  zeros (nl , n2 , n3 ) ; 

% 

db2 

=  zeros (nl,n2,n3) ; 

% 

db3 

=  zeros (nl , n2 , n3 ) ; 

%n  = 

length (x)  *  length (y)  *  length (z) 

% 

dbl 

=  zeros (n, 1 ) ; 

% 

db2 

=  zeros (n, 1) ; 

% 

db3 

=  zeros (n, 1) ; 

% 

db  = 

:  zeros (n, 1 ) ; 

%% 

db  = 

:  dbl  +  db2  +  db3; 

%% 

dbl 

=  db  /  3; 

%% 

db2 

=  dbl; 

%% 

db3 

=  dbl; 

sizes  ? 


C.Rao 

sigma3D/func_h.m 


function  [hl,h2]  =  func_h (x, y , z ) 


%  usage 

:  [hleh2]  =  func_h(Xey, z) 

%  where 

x,yeZ  and  hi , h2  are  of  the  same  size 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% 

The  observation  function 

%% 

Chuanxia  Rao 

%% 

April  1997 

%% 

May  1997 

%% 

Jul  1997 

%%  - 

%% 

Called  in: 

%% 

obs_cor .m 

%% 

generate .m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

if  {size(x)==size{y)  &  size (y) ==size ( z)  )  , 

[ll,min,nn]  =  size(y); 

for 

for  j  =  l:min, 
for  k=l:nn, 

templ2  =  x(i,j/k)^2  +  y(io,k)"^2; 
if  templ2==0, 

hi (i, j ,k)  =  0; 

h2(i,j,k)  =  asin(  sign (z ( i , j , k) )  ); 

else 

argl  =  x{i,j/k)  /  sqrt ( templ2 ) ; 

tempi  =  sign(y(i, j,k) )  *  acos{argl); 

hl{i,j,k)  =  temple- 

temp  =  templ2  +  z(i,jek)'^2; 

arg2  =  z{i,jek)  /  sqrt{temp); 

h2(iejek)  =  asin(arg2); 

end 

end 

end 

end 

else 


end 


disp('  Warning:  x,  y,  and  z  have  different  sizes  ?!') 


C.Rao 

generate.m 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Generate  a  3D  trajectory  for 

%%  dX  =  b(X)dt  +  siginaa*dW 

%%  X(0)  =  XO 

%%  And  generate  an  observation  for 

z (k)  =  h(X(t_k))  +  delta*v(k) 

%%  Chuanxia  Rao 

%%  January  1996 

%%  January  1997 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%%  Dependencies: 

%%  initialize .m; 

%%  func_b.m  (function) 

%%  func_h.m  (function) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%  Output: 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%  initialize; 

disp( '  Generating  the  trajectory  ...'); 

%out_fileZ  =  'data_Z.m'; 

%out_fileX  =  'data_X.m'; 

%Kfinal  =  Kt; 
srdt  =  sqrt(dt); 

X ( 1 , : )  =  XO ; 

Z(l, : )  =  ZO; 


randn ( ' seed' ,  0 ) ; 

V  =  randn(Kfinal, 2 ) ; 

Xk  =  X(l, : ) ; 
for  k  =  l:Kfinal, 

kl  =  k  +  1; 

%  [bkl,bk2,bk3]  =  func_b (Xk ( 1 ) , Xk ( 2 ) , Xk ( 3 ) )  ; 

%  bk  =  [bkl,bk2,bk3] ; 

%  temp  =  Xk  +  dt*bk;  %%  forward  Euler 

%  [bkl,bk2,bk3]  =  func_b ( temp ( 1 ) ,  temp (2),  temp (3)); 

%  b2  =  bk  +  [bkl,bk2,bk3] ; 

%  temp  =  Xk  +  dt2  *  b2 ;  %%  trapezoid 

%  tempi  =  srdt*diag(sigmaa) *randn(dim, 1) ; 

%  X(kl,:)  =  temp  +  tempi'; 

[xl,x2,x3]  =  advection(dt,  Xk(l) ,Xk(2) ,Xk(3) ) ; 

tempi  =  srdt  *  sigmaa  .*  randn ( 1 , dim) ;  %randn (size (sigmaa) ) ; 

X(kl,:)  =  [xl,x2,x3]  +  tempi; 

%  [T,Y]  =  ode23 ( ' func„fb' ,  [0  dt] ,  [Xk ( 1 ) , Xk ( 2 ) , Xk ( 3 ) ] ) ; 

%  [mY,nY]  =  size(Y); 

%  X(kl,:)  =  Y(mY, :)  +  tempi; 

Xk  =  X(kl, : ) ; 

[hl,h2]  =  func_h(Xk(l) ,Xk(2) ,Xk(3) ) ; 
temp2  =  diag(delta)*v(k,:)'; 

Z(kl,:)  =  [hl,h2]  +  temp2 ' ; 

end 

%% 

%%  Output  --  graph: 

%% 


kl  =  1:  (Kfinal  +  1)  ; 
kll  =  (l:Kfinal)+l; 
tl  =  0:dt:Tfinal; 


C.Rao 

generate.m 


til  =  dt : dt : Tf inal  ; 

al  =  min(X(kl, 1) ) ;  bl  =  max{X(kl, 1) ) ; 
a2  =  min(X(kl, 2) ) ;  b2  =  max (X (kl , 2 ) ) ; 
a3  =  min(X{kl, 3) ) ;  b3  =  max (X (kl , 3 ) ) ; 
aZl  =  min(Z (kll, 1) ) ;  bZl  =  max (Z (kll , 1 ) ) ; 
aZ2  =  min(Z(kll,2) ) ;  bZ2  =  max(Z (kll , 2 ) ) ; 

figure ; 

subplot (221) ,  plot(tl,X(kl,l)); 
axis ( [0,Tf inal,  al,bl]); 
subplot (222) ,  plot(tl,X(kl,2)); 
axis ( [0 , Tf inal ,  a2,b2]); 
subplot (223) ,  plot(tl,X(kl,3)); 
axis( [0,Tfinal,  a3,b3]); 

subplot(224) ,  plot3 (X(kl,3) ,  X(kl,l),  X(kl,2),  'r'); 

axis([a3,b3,  al,bl,  a2,b2]); 

figure; 

subplot(211) ,  plot(tll,Z(kll,l) ) ; 
axis ( [0,Tf inal,  aZl,bZl]); 
subplot (212) ,  plot ( til, Z (kll, 2) ) ; 
axis ( [0 , Tf inal ,  aZ2,bZ2]); 

clear  temp  tempi  temp2 
clear  bk  bkl  bk2  bk3 
clear  hi  h2 
clear  v  srdt 
clear  al  a2  a3  aZl  aZ2 
clear  bl  b2  b3  bZl  bZ2 
clear  kl  kll 
clear  tl  til 


C.Rao 

onestep.m 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^^^^^^^^^^^^^^ 
computation  of  the  UFD  for  the  filtering  problem: 
%%  dX  =  b(X)dt  +  sigmaa(X)dW,  X(0)  --  N(mO,varO) 

Z(k)  =  h(X(t_k))  +  delta (k)v(k) 

%% - 


%%  Chuanxia  Rao 

%%  Jul  1997 

%%  Mar  1997 

%%  Feb  1996 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%^% 


%%  Dependencies: 

%%  initialize. m 

%%  moreinit.m 

%%  obs_cor,m 

%%  prior. m 

%%  cutsmall.m 

%%  generate. m  (for  observations  Z) 

%% - 

%%  Output: 

%%  posterior  density  pkl  (denoted  as  pk) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%%initialize; 

%%moreinit ; 

%%  —  They  must  be  called  before  running  this  file. 


% 


% 


[xx,yy,zz,  pk]  =  prior (xx, yy, zz ,  pk) ; 

Tpk  =  cutsmall (Tpk) ; 

%%  --  prior  density  Tpk 

alpha_kl  =  obs_cor (Z (kl , 1) , Z (kl , 2 ) ,  xx,yy,zz); 
alpha_kl  =  cutsmall (alpha_kl) ; 

%%  --  corrector  alpha_kl 

pk  =  alpha_kl  .*  Tpk; 
pk  =  alpha_kl  .*  pk; 

pk_max  =  max (  max (  max (pk,  [  ]  ,  3 ) ,  [  ] , 2  ),[],!  ) ; 
if  pk_max  >  0, 
pk  =  pk/pk_max; 
end 

pk  =  cutsmall (pk); 


%clear  alpha_kl  Tpk 
clear  alpha_kl 


%%  —  density  at  step  kl 


C.Rao 
prior.m 

[x_new,y_new, z_new,  p_new]  =  prior (x_old,y_old, z_old,  p_old) 
[x_new,y_new, z_new,  p_new]  =  prior (x_old, y_old, z_old,  p_old) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Chuanxia  Rao 

%%  July  1997 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%  Dependencies: 

%%  advection.m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

global  dt  dt2  denom 

[x_new,  y_new,  z__new]  =  advection (dt ,  x_old,  y_old,  z_old)  ; 

temp  =  -  func_db  (x__new,  y_new,  z_new)  ; 
temp  =  temp  -  func__db  {x_old,  y__old,  z_old)  ; 
temp  =  dt2  *  temp; 
p_new  =  exp (  temp  ) ; 
p_new  =  p_old  . *  p_new; 

[ml, m2, m3]  =  size  (p__new)  ; 
mlml  =  ml-1; 
m2ml  =  m2-l; 
m3 ml  =  m3-l; 

x_old  =  x_new ( 2 : ml ,  : ,  : )  ; 

temp  =  exp(- (x_old-x_new(l;ralml,  :  ,  :  )  )  .'^2/denom(l)  )  ; 
temp  =  temp  .*  p_new(2:ml,:,:); 
p_new(l:mlml,  :  ,  :  )  =  p„new  ( 1  :mlml ,  :  ,  :  )  +  temp; 
x_old  =  x_new(l:mlml,  :  ,  :  )  ; 

temp  =  exp  (~  (x_old-x__new (2  :ml,  ■^2/denom{l)  )  ; 

temp  =  temp  p_new(l:mlml,:,:); 
p_new(2 :ml, : , : )  =  p_new ( 2 :ml , : , : )  +  temp; 

y_old  =  y_new( : ,2:m2, : ) ; 

temp  =  exp{- (y_old-y_new(  :  ,  l:m2ml,  :  )  )  .^2/denom{2)  )  ; 

temp  =  temp  . *  p_new { : , 2 : m2 , : ) ; 

p_new  (  :  ,  1 :  m2ml ,  :  )  =  p_new  (  :  ,  1 :  m2ml ,  :  )  +  temp  ; 

y_old  =  y_new{  :  ,  1  :m2ml,  :  )  ; 

temp  =  exp(-(y_old-y_new{  :  ,2:m2,  :  )  )  .^2/denom(2)  )  ; 

temp  =  temp  .  *  p_new{  :  ,  1  :m2ml,  :  )  ; 

p_new ( : , 2 :m2 , : )  =  p_new ( : , 2 :m2 , : )  +  temp; 

z_old  =  z_new ( : , : , 2 : m3 ) ; 

temp  =  exp(-{z_old-z_new( ; , : , l:m3ml) ) .^2/denom(3) ) ; 

temp  =  temp  .  *  p__new  (  :  ,  :  ,  2  :  m3  )  ; 

p_new  {  :  ,  :  ,  1 : m3ml )  =  p_new  {  :  ,  :  ,  1 : m3ml )  +  temp ; 

z_old  =  z_new( : , : , 1 :m3ml) ; 

temp  =  exp(-{z_old-z_new{ : , : ,2:m3) ) .^2/denom{3) ) ; 

temp  =  temp  .  *  p_new (  :  ,  :  ,  1  :m3ml )  ,- 

p_new ( : , : , 2 : m3 )  =  p_new ( : , : , 2 : m3 }  +  temp ; 

%  [praax,  i,j,k]  =  max{p_new) ; 

%  p_new  =  p__new  /  pmax; 

%  star  =  [x_new(i, j ,k) ,  y_new ( i , j , k) ,  z_new { i , j , k) ] ; 

%  x_star  =  [x_star'  star']'; 


function 
%  usage : 


%return 


function 
%  usage: 
%  where 
% 

% 

% 


obs_cor.m 

result  =  obs_cor (zl, z2,  x,y,z) 
result  =  obs_cor (zl, z2 ,  x,y,z) 
zl,z2  are  the  measurements,  and 
(x,y,z)  are  the  spatial  points. 

%hhl ,  hh2  are  the  noise- free  part, 
dim  =  3 ;  dim_obs  =  2 ; 


%% 

Chuanxia  Rao 

%% 

July  1997 

%% 

May  1997 

%% 

Feb  1996 

%% 

Used  in:  onestep.m 

global 

denom_obs  %%  defined  in 

initialize  .m 


[hhl,  hh2]  =  func_h(x,  y,  z) ; 
temp  =  zl  -  hhl; 

tempi  =  temp/denom_obs (1)  .*  temp; 

temp  =  z2  -  hh2; 

tempi  =  tempi  +  temp/denom_obs ( 2 )  .*  temp; 

tempi  =  tempi  -  min (  min (  min ( tempi ,[],3),[],2  ),[],!  ) 

result  =  exp{  -  tempi  ); 


peak.m 


function  xyz  =  peak{pk) 

%usage:  xyz  =  peak(pk) 

%  where  size(pk)  =  [nxpl  nypl  nzpl] 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%^^^^^^^^ 
%% 

%%  This  is  for  plotting  the  tracking  process. 

%%  Chuanxia  Rao 

%%  April  1997 

%%  May  1997 

%% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%|,% 

global  XX  yy  zz 

[one,  ijk]  =  max (pk,  [ ] , 3 )  ; 

[one,  ij]  =  max(one,  [] ,2)  ; 

[ one ,  i ]  =  max ( one ,  [],!); 

j  =  i  j  { i )  ; 
k  =  ijk(i, j)  ; 

X  =  xx(i, j  ,k)  ; 
y  =  yy(i, j ,k)  ; 
z  =  zz(i, j,k) ; 
xyz  =  [x,y, z] ' ; 

return 


function 
%  usage: 


[  x_new , y_new , z_new ] 
[  x_new ,  y__new ,  z_new  ] 


C.Rao 

advection.m 

advection(dtO,  x_old,y_old, z_old) 
advection(dtO ,  x_old,y_old, z_old) 


%%  Chuanxia  Rao 

%%  Februry  1997 

%%  April  1997 

%%  June  1997 


global  substeps 


dt  =  dtO /substeps ; 
%dt6  =  dt/6; 
dt2  =  dt/2; 

X  =  x_old; 
y  =  y_old; 
z  =  z_old; 


for  k  =  1: substeps, 

[bkl , bk2 , bk3 ]  =  func_b (x, y, z ) ; 

[bl , b2 , b3 ]  =  f unc_b (x+dt *bkl , y+dt*bk2 , z+dt *bk3 ) ; 

X  =  X  +  dt2  *  (bkl  +  bl)  ; 

y  =  y  +  dt2  *  {bk2  +  b2 ) ; 

z  =  z  +  dt2  *  (bk3  +  b3 ) ;  %%  trapezoid 

%  bk  =  f unc_b ( x ) ; 

%  temp  =  func_b(x  +  dt2*bk) ; 

%  X  =  X  +  dt  *  temp;  %%  modified  Euler 


%% 

kl  = 

func_b (x) ; 

%% 

k2  = 

func_b{x  + 

dt2*kl) ; 

%% 

k3  = 

func_b{x  + 

dt2*k2) ; 

%% 

k4  = 

func_b(x  + 

dt*k3) ; 

%% 

temp 

=  kl  +  2*k2 

+  2*k3  +  k4; 

%%  X  =  X  +  dt6  *  temp; 

end 


x_new  =  x; 
y_new  =  y; 
z_new  =  z ; 


%%  Runge-Kutta-4 


C.Rao 

cutsmall.m 


function  result  =  cutsmall (array) 

%  usage:  result  =  cutsmall (array) 

%%  Chuanxia  Rao 

%%  May  1997 

%%  Used  in:  onestep.m 

global  small  %%  defined  in  initialize. m 

result  =  (array  >  small)  array; 

%  result  =  sparse {  result  ); 


function 
%usage : 

% 

% 


C.Rao 

plotting.m 

plotting (xyz ,  pk) 
plotting (xyz ,  pk) 
where  length (xyz)  =  dim (=3) 

size(pk)  =  [nxpl  nypl  nzpl] 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%% 

%%  This  is  for  plotting  the  tracking  process. 

%%  Chuanxia  Rao 

%%  April  1997 

%%  May  1997 

%% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

global  xendO  yendO  zendO 
global  xendl  yendl  zendl 
global  XX  yy  zz 
global  resol_vert 

X  =  xyz ( 1 ) ; 
y  =  xyz { 2 ) ; 
z  =  xyz { 3 ) ; 

plots (z,  X,  y,  'r* ' ) ; 

axis([zendO  zendl  xendO  xendl  yendO  yendl]); 
%  axis ( [xendO  xendl  yendO  yendl  zendO  zendl]); 

%  hold  on; 

[one,  ijk]  =  max (pk, [ ] , 3 ) ; 

[one,  ij]  =  max (one,  [  ] , 2 )  ; 

[ one ,  i ]  =  max ( one , [],!); 

3  =  ij (i)  ; 
k  =  ijk{i, j )  ; 
x  =  XX ( i , j , k ) ; 

y  =  yy{i. j/k) ; 

z  =  zz (i, j , k) ; 

%  plots (x,  y,  z,  'ro',  ' LineWidth ' , [2 ] ) ; 

plots (z,  X,  y,  'g*'); 
grid  on; 

drawnow;  %pause ( 1 ) ; 

%  hold  off; 


return 


